﻿using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using System.Collections;
using GHIElectronics.NETMF.FEZ;
using GHIElectronics.NETMF.Hardware;
using GHIElectronics.NETMF.IO;
using Microsoft.SPOT.IO;
using System.IO;
using System.Text;
using Devantech.Hardware.SD20;

namespace MLRobotic
{
    public class Program
    {
        //Button used to send data
        private static InputPort bouton = new InputPort((Cpu.Pin)GHIElectronics.NETMF.Hardware.USBizi.Pin.IO0, false, Port.ResistorMode.PullUp);
        public static OutputPort led;
        public static CAN_Communication CAN1;
        public static SD20ServoDriver sd21;


        public static void Main()
        {

            //Init the I/O
            Init();
            InitDesObjectifsPincipaux();
            //Wait(500);
            //Program.CAN1.Send(CAN_Communication.CANId.DemandeOuvertureChariotFermeture);
            //Wait(500);
            //Thread.Sleep(1000);
            Log.Write("Program begin");
            //Ecran.Print("Hello World", true);

            //Balise.isActif = false;
                      

        

            // Init du tableau de positions a atteindre
            //Position.AddTarget(new Position(10000+Robot.ConvertCMToPas(40),10000));
            
            
            //Position.AddTarget(new Position(14000, 10000));
            //Position.AddTarget(new Position(14000, 14000));
            //Position.AddTarget(new Position(10000, 14000));
            //Position.AddTarget(new Position(10000, 10000));
            //Position.AddTarget(new Position(0));
            
            ////Carre
            //Position.AddTarget(new Position(10000, 14000));
            //Position.AddTarget(new Position(14000, 14000));
            //Position.AddTarget(new Position(14000, 10000));
            //Position.AddTarget(new Position(10000, 10000));
             //Position.AddTarget(new Position(13333, 11999));
            //Position.AddTarget(new Position(Robot.ConvertCMToPas(70)+10000, uint.MaxValue));
            //Position.AddTarget(new Position(10000, 14000));
            //Position.AddTarget(new Position(10000, 10000));
            //Position.AddTarget(new Position(0));


            //Position.AddTarget(new Position(15000, 15000));
            //Position.AddTarget(new Position(10000, 15000));
            //Position.AddTarget(new Position(10000, 10000));
            //Position.AddTarget(new Position(0));



            //// attente de relache bouton
            //while (!buttonStop.Read())
            //{
            //    // Process des messages CAN
            //    CAN1.ProcessCurrentMessages();
            //}

            //// attente de relache bouton
            //while (buttonStop.Read())
            //{
            //    // Process des messages CAN
            //    CAN1.ProcessCurrentMessages();
            //}

            
                DateTime HeureDefin = DateTime.Now.AddSeconds(88);
                while (DateTime.Now < HeureDefin)
                {
                    // Process des messages CAN
                    CAN1.ProcessCurrentMessages();

                    // Process du deplacement
                    Position.ProcessDirection();

                    // Process de la Balise
                    //Balise.ProcessBalise();
                    //if (!Robot.Arretetoi)
                        IA.Process();

                    Capteurs.Process();

                    //Ecran.Process();

                    //RoutineCalculPID();

                    //Capteurs.Process();

                    // Bouton Stop
                    //if (!buttonStop.Read())
                    //    Position.Stop();


                    // Test du capteur telemetre
                    //serial.CursorHome();
                    //serial.Print(capteur.Read().ToString() + "    ");

                    // Tes d'envois de messages CAN
                    //TestCanSend();

                    // Sleep pour tests
                    //Thread.Sleep(250);
                }
                Position.Stop();
        }


        private static void Wait(int milliseconde)
        {
            for (int i = 0; i < milliseconde / 10; i++)
            {
                Thread.Sleep(10);
                CAN1.ProcessCurrentMessages();
            }
        }

        public static short currentP = 30;
      public static short currentI = 28;
      public static short currentD = 0;
      
        private static void RoutineCalculPID()
        {
            if (Robot.ActionEnCours == Robot.TypeActionEncours.Arret && currentP < 35)
            {
                //currentP = (short)(currentP + 5);
                Thread.Sleep(500);
                CAN1.Send((int)10000, CAN_Communication.CANId.SetPositionX);
                CAN1.Send((int)10000, CAN_Communication.CANId.SetPositionY);
                CAN1.Send((int)0, CAN_Communication.CANId.SetPositionTeta);
                Position.InitPID(currentP, currentI, currentD, CAN_Communication.CANId.InitPid1);
                Position.InitPID(currentP, currentI, currentD, CAN_Communication.CANId.InitPid2);

                Thread.Sleep(100);
                Log.Write("NEW_ASSERV "+ currentP.ToString() + "_" + currentI.ToString() + "_" + currentD.ToString());

                
                //une fois en avant, une fois en arrière
                Position.AddTarget(new Position(15000, 10000));
                //Position.AddTarget(new Position(3.1));
                Thread.Sleep(500);
            }
        }

        /// <summary>
        /// Init
        /// </summary>
        private static void Init()
        {
            Debug.EnableGCMessages(false);
            //Log.forceDebug = true;
            Log.Init();
            Log.Write("Init");
            Robot.Machoire = Robot.EtatMachoire.Desactivee;
            Balise.isActif = false;
            bool initError = false;

      
            #region CAN
            try
            {
                //Init the CAN Communication
                CAN1 = new CAN_Communication(CAN.Channel.Channel_1, 250);

            }
            catch (Exception ex)
            {
                //Log.Write(ex.Message);
                Log.Write("Erreur Init CAN : " + ex.Message);
                initError = true;
            }
            #endregion 

            #region LCD
            try
            {
                Ecran.Init();
            }
            catch (Exception ex)
            {
                //Log.Write(ex.Message);
                Log.Write("Erreur Init LCD : " + ex.Message);
                initError = true;
            }
            #endregion


            #region SD21
            // init du SD21
            //sd21 = new SD20ServoDriver();
            #endregion


            Capteurs.Init();

            
            while (!Robot.AlerteRebootCarteAsserv)
            {
                Thread.Sleep(10);
                CAN1.ProcessCurrentMessages();

            }

            InitCarteAsservissemnent();
            
            IA.OuvertureCharriot(Robot.EtatCharriot.OuvertGauche);

            while (Robot.Charriot != Robot.EtatCharriot.OuvertGauche)
            {
                Thread.Sleep(10);
                CAN1.ProcessCurrentMessages();
            }
            

            IA.FermetureCharriot();

            //Robot.couleur = Capteurs.RegarderCouleurSousLesCapteurs();
            //Log.Write("Couleur du robot :"+Robot.couleur.ToString());
            /*while (Robot.Charriot != Robot.EtatCharriot.Fermee)
            {
                Thread.Sleep(10);
                CAN1.ProcessCurrentMessages();
            }*/
            Log.Write("Charriot ferme");

            #region led de debug
            //led use to chack when data is sent
            led = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.LED, false);

            if (initError)
            {
                while (true)
                {
                    led.Write(true);
                    Thread.Sleep(200);
                    led.Write(false);
                    Thread.Sleep(200);
                }

            }
            #endregion

            //Log.Write("Couleur du robot :" + Robot.couleur.ToString());

            #region Gestion jack
            Log.Write("Attente Jack");
            while (Capteurs.JackDemarrageDedans())
            {
                Thread.Sleep(10);
                CAN1.ProcessCurrentMessages();
                if (Capteurs.InterrupteurCouleur.Read())
                    Robot.couleur = Capteurs.Couleur.Bleu;
                else
                    Robot.couleur = Capteurs.Couleur.Rouge;
                //
            }

            Log.Write("Jack ok");
            Log.Write("Couleur du robot R=0 et B=1 -->" + Robot.couleur.ToString());
            #endregion

            AireDeJeu.Init();
        }
            

        /// <summary>
        /// Procedure d'initialisation de la carte d'assservissement lorsque elle demarre ou redemarre
        /// </summary>
        public static void InitCarteAsservissemnent()
        {
            Log.Write("Init de la carte d'asserv");
            //Init des valeurs du PID
            Position.InitPID(35, 28, 0, CAN_Communication.CANId.InitPid1);
            Position.InitPID(35, 28, 0, CAN_Communication.CANId.InitPid2);
            
            
            Position.InitEntraxe(2710,2080);
            Position.InitPenteEtVmax(20, 2048);

            //Envoi de la derniere position connue
            CAN1.Send(Robot.currentPosition.X, CAN_Communication.CANId.SetPositionX);
            CAN1.Send(Robot.currentPosition.Y, CAN_Communication.CANId.SetPositionY);
            CAN1.Send(Robot.currentPosition.Teta, CAN_Communication.CANId.SetPositionTeta);

            Robot.AlerteRebootCarteAsserv = false;

        }
        

        /// <summary>
        /// Envoi un message CAN à chaque appui sur le boutton de la carte
        /// </summary>
        private static void TestCanSend()
        {
            
            //Some dummy data
            string[] lstString = new string[] { "Test", "Hippo", "Poney", "Renard", "Hipp", "Rien" };
            int currentStr = 0;

            if (!bouton.Read())
            {
                // increment index
                currentStr = (currentStr + 1) % lstString.Length;

                CAN1.SendString(lstString[currentStr]);
                Log.Write("Sended :" + lstString[currentStr]);

                //blink the led after the sending
                MLRobotic.Program.led.Write(true);
                Thread.Sleep(150);
                MLRobotic.Program.led.Write(false);

                // wait for the button to be pull of
                while (!bouton.Read())
                    Thread.Sleep(50);
            }
        }

        private static void InitDesObjectifsPincipaux()
        {
            

            #region BLeu// ajout des objectifs spécifiques à l homologation
            if (Robot.couleur == Capteurs.Couleur.Bleu)
            {
                //Position.InitEntraxe(1900);
                uint PosXDepart = (Robot.ConvertCMToPas(300- (30 - (Robot.LongueurRobot / 2))) + 10000);
                uint PosYDepart = (Robot.ConvertCMToPas(35 - (Robot.LargeurRobot / 2)) + 10000);
                double PosTetatDepart = - System.Math.PI;

                CAN1.Send(PosXDepart, CAN_Communication.CANId.SetPositionX);
                Robot.currentPosition.X = PosXDepart;
                CAN1.Send(PosYDepart, CAN_Communication.CANId.SetPositionY);
                Robot.currentPosition.Y = PosYDepart;
                CAN1.Send(PosTetatDepart, CAN_Communication.CANId.SetPositionTeta);
                Robot.currentPosition.Teta = PosTetatDepart;

                IA.ListObjectifsPrincipaux.Enqueue(new Position(PosXDepart - Robot.ConvertCMToPas(30), PosYDepart));
                IA.ListObjectifsPrincipaux.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(5, 1));
                IA.ListObjectifsPrincipaux.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(5, 2));
                IA.ListObjectifsPrincipaux.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(5, 3));
                IA.ListObjectifsPrincipaux.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(5, 4));
                IA.ListObjectifsPrincipaux.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(5, 5));

                IA.ListObjectifsSecondaire.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(4, 4));
                IA.ListObjectifsSecondaire.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(4, 3));
                IA.ListObjectifsSecondaire.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(4, 2));
                IA.ListObjectifsSecondaire.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(4, 1));

            }
            #endregion

            #region Rouge
            else
            {
                //Position.InitEntraxe(2580);
                uint PosXDepart = (Robot.ConvertCMToPas(30 - (Robot.LongueurRobot / 2))+10000);
                uint PosYDepart = (Robot.ConvertCMToPas(35 - (Robot.LargeurRobot / 2)) + 10000);
                double PosTetatDepart = 0;
                
                
                ////Init des positions
                CAN1.Send(PosXDepart, CAN_Communication.CANId.SetPositionX);
                Robot.currentPosition.X = PosXDepart;
                CAN1.Send(PosYDepart, CAN_Communication.CANId.SetPositionY);
                Robot.currentPosition.Y= PosYDepart;
                CAN1.Send(PosTetatDepart, CAN_Communication.CANId.SetPositionTeta);
                Robot.currentPosition.Teta=PosTetatDepart;
                

                IA.ListObjectifsPrincipaux.Enqueue(new Position(PosXDepart+Robot.ConvertCMToPas(30),PosYDepart));
                IA.ListObjectifsPrincipaux.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(1, 1));
                IA.ListObjectifsPrincipaux.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(1, 2));
                IA.ListObjectifsPrincipaux.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(1, 3));
                IA.ListObjectifsPrincipaux.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(1, 4));
                IA.ListObjectifsPrincipaux.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(1, 5));


                IA.ListObjectifsSecondaire.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(2, 4));
                IA.ListObjectifsSecondaire.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(2, 3));
                IA.ListObjectifsSecondaire.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(2, 2));
                IA.ListObjectifsSecondaire.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(2, 1));
                //IA.ListObjectifsPrincipaux.Enqueue(new Position(10000,11000));
                //IA.ListObjectifsPrincipaux.Enqueue(new Position(11000, 11000));

                //IA.ListObjectifsPrincipaux.Enqueue(new Position(6 * System.Math.PI));
                //IA.ListObjectifsPrincipaux.Enqueue(AireDeJeu.PositionEnPasDeLIntersection(1,1));
                //IA.ListObjectifsPrincipaux.Enqueue(new Position(Robot.ConvertCMToPas(60)+10000, Robot.ConvertCMToPas(60)+10000));
                //IA.ListObjectifsPrincipaux.Enqueue(new Position(Robot.ConvertCMToPas(60) + 10000, Robot.ConvertCMToPas(90) + 10000));
                //IA.ListObjectifsPrincipaux.Enqueue(new Position(Robot.ConvertCMToPas(75) + 10000, Robot.ConvertCMToPas(150) + 10000));

            }
            #endregion

        }
    }
}
